package com.guazi.trackingtest.cal.op;

import android.util.Log;

import com.guazi.trackingtest.cal.data.Point3D;

public class Adjust {

    public static Point3D adjust(Point3D acceleration, Point3D angle) {

//		double s1 = Math.sin(angle.x);
//		double c1 = Math.cos(angle.x);
//
//		double s2 = Math.sin(angle.y);
//		double c2 = Math.cos(angle.y);
//
//		double s3 = Math.sin(angle.z);
//		double c3 = Math.cos(angle.z);
//
//		double x = acceleration.x * ( c3 * c1 - s3 * s2 * s1 )
//				 + acceleration.y * ( - s3 * c2 )
//				 + acceleration.z * ( s1 * c3 + c1 * s2 * s3 );
//
//		double y = acceleration.x * ( c3 * s2 * s1 + s3 * c1 )
//				 + acceleration.y * ( c3 * c2 )
//				 + acceleration.z * ( s1 * s3 - c1 * s2 * c3 );
//
//		double z = acceleration.x * ( - c2 * s1 )
//				 + acceleration.y * ( s2 )
//				 + acceleration.z * ( c1 * c2 );

//        xzy
//        double s1 = Math.sin(angle.x);
//        double c1 = Math.cos(angle.x);
//
//        double s2 = Math.sin(angle.z);
//        double c2 = Math.cos(angle.z);
//
//        double s3 = Math.sin(angle.y);
//        double c3 = Math.cos(angle.y);
//
//        double x = acceleration.x * (c2 * c3)
//                + acceleration.y * ( -s2 )
//                + acceleration.z * (c2 * s3);
//
//        double y = acceleration.x * (s1 * s3 + c1 * c3 * s2)
//                + acceleration.y * (c1 * c2)
//                + acceleration.z * (c1 * s2 * s3 - c3 * s1);
//
//        double z = acceleration.x * (c3 * s1 * s2 - c1 * s3)
//                + acceleration.y * (c2 * s1)
//                + acceleration.z * (c1 * c3 + s1 * s2 * s3);

//        xyz
//        double s1 = Math.sin(angle.x);
//        double c1 = Math.cos(angle.x);
//
//        double s2 = Math.sin(angle.y);
//        double c2 = Math.cos(angle.y);
//
//        double s3 = Math.sin(angle.z);
//        double c3 = Math.cos(angle.z);
//
//        double x = acceleration.x * (c2 * c3)
//                + acceleration.y * ( -c2 * s3 )
//                + acceleration.z * (s2);
//
//        double y = acceleration.x * (c1 * s3 + c3 * s1 * s2)
//                + acceleration.y * (c1 * c3 - s1 * s2 * s3)
//                + acceleration.z * (-c2 * s1);
//
//        double z = acceleration.x * (s1 * s3 - c1 * c3 * s2)
//                + acceleration.y * (c3 * s1 + c1 * s2 * s3)
//                + acceleration.z * (c1 * c2);

//      yxz
//        double s1 = Math.sin(angle.y);
//        double c1 = Math.cos(angle.y);
//
//        double s2 = Math.sin(angle.x);
//        double c2 = Math.cos(angle.x);
//
//        double s3 = Math.sin(angle.z);
//        double c3 = Math.cos(angle.z);
//
//        double x = acceleration.x * (c1 * c3 + s1 * s2 * s3)
//                + acceleration.y * (c3 * s1 * s2 - c1 * s3)
//                + acceleration.z * (c2 * s1);
//
//        double y = acceleration.x * (c2 * s3)
//                + acceleration.y * (c2 * c3)
//                + acceleration.z * (-s2);
//
//        double z = acceleration.x * (c1 * s2 *s3 - c3 * s1)
//                + acceleration.y * (c1 * c3 * s2 + s1 * s3)
//                + acceleration.z * (c1 * c2);

//        yzx
//        double s1 = Math.sin(angle.y);
//        double c1 = Math.cos(angle.y);
//
//        double s2 = Math.sin(angle.z);
//        double c2 = Math.cos(angle.z);
//
//        double s3 = Math.sin(angle.x);
//        double c3 = Math.cos(angle.x);
//
//        double x = acceleration.x * (c1 * c2)
//                + acceleration.y * (s1 * s3 - c1 * c3 * s2)
//                + acceleration.z * (c3 * s1 + c1 * s2 * s3);
//
//        double y = acceleration.x * (s2)
//                + acceleration.y * (c2 * c3)
//                + acceleration.z * (-c2 * s3);
//
//        double z = acceleration.x * (-c2 * s1)
//                + acceleration.y * (c1 * s3 + c3 * s1 * s2)
//                + acceleration.z * (c1 * c3 - s1 * s2 * s3);

////        zyx
//        double s1 = Math.sin(angle.z);
//        double c1 = Math.cos(angle.z);
//
//        double s2 = Math.sin(angle.y);
//        double c2 = Math.cos(angle.y);
//
//        double s3 = Math.sin(angle.x);
//        double c3 = Math.cos(angle.x);
//
//        double x = acceleration.x * (c1 * c2)
//                + acceleration.y * (c1 * s2 * s3 - c3 * s1)
//                + acceleration.z * (s1 * s3 + c1 * c3 * s2);
//
//        double y = acceleration.x * (c2 * s1)
//                + acceleration.y * (c1 * c3 + s1 * s2 * s3)
//                + acceleration.z * (c3 * s1 * s2 - c1 * s3);
//
//        double z = acceleration.x * (-s2)
//                + acceleration.y * (c2 * s3)
//                + acceleration.z * (c2 * c3);

//        zxy
        double s1 = Math.sin(angle.z);
        double c1 = Math.cos(angle.z);

        double s2 = Math.sin(angle.x);
        double c2 = Math.cos(angle.x);

        double s3 = Math.sin(angle.y);
        double c3 = Math.cos(angle.y);

        double x = acceleration.x * (c1 * c3 - s3 * s2 * s1)
                + acceleration.y * (-c2 * s1)
                + acceleration.z * (c1 * s3 + c3 * s1 * s2);

        double y = acceleration.x * (c3 * s1 + c1 * s2 * s3)
                + acceleration.y * (c1 * c2)
                + acceleration.z * (s1 * s3 - c1 * c3 * s2);

        double z = acceleration.x * (-c2 * s3)
                + acceleration.y * (s2)
                + acceleration.z * (c2 * c3);

//        Log.e("lqy", "Z ----------->" + acceleration.z);
//        Log.e("lqy", "x ----------->" + x);
//        Log.e("lqy", "y ----------->" + y);
        Log.e("lqy", "z ----------->" + z);

        return new Point3D(x, y, z);
    }
}
